#ifndef LIDAR_CELLS_H_
#define LIDAR_CELLS_H_

#include "algorithm.h"
#include "classify/classify.h"
#include "cluster/DBSCAN_kdtree.h"
#include "common/algorithm_type/all_type.h"
#include "common/data_pool.h"
// #include "common/log.h"
#include "classify/obb.h"
#include "cluster/euclidean_cluster.hpp"
#include "lidar_cells_grid.h"
#include "rclcpp/rclcpp.hpp"
#include "track/track.h"
#include <iomanip>
#include <math.h>

// #include "../lidar_cell_objs/lidar_cell_objs.h"
namespace perception
{
    namespace algorithm
    {
        class LidarCells
        // class ConditionalFilter
        {
        public:
            // using Ptr = pcl::PointCloud<T1>::Ptr;
            // using ConstPtr = pcl::PointCloud<T1>::ConstPtr;
            LidarCells(cell_config &config, rclcpp::Node *private_nh)
                : private_nh_(private_nh), cell_config_(config)
            {
                // private_nh_->declare_parameter<int>("hz", hz);
                private_nh_->get_parameter("hz", hz);
                this->row = (int)(config.xmax - config.xmin) / config.cell_size_x;
                this->col = (int)(config.ymax - config.ymin) / config.cell_size_y;
                data_cell_ = std::vector<std::vector<data_cell>>(
                    this->row, std::vector<data_cell>(this->col));
                data_cell_tmp = std::vector<std::vector<data_cell>>(
                    this->row, std::vector<data_cell>(this->col));
                lidar_cells_grid_ = std::make_shared<LidarCellsGrid>(config);
                classify_ = std::make_shared<Classify>();
                // dbscan_ = std::make_shared<DBSCAN>(
                //     config.dbscan_config_.z_eps, config.dbscan_config_.min_points_num,
                //     config.dbscan_config_.search_scope, config);
                track_ = std::make_shared<Track>(config.track_config_, private_nh);
                for (int i = 0; i < row; i++)
                {
                    for (int j = 0; j < col; j++)
                    {
                        data_cell_[i][j].reset();
                        data_cell_tmp[i][j].reset();
                    }
                }
            }
            ~LidarCells() {}
            int Process(LidarPoint &data_in, Localization &loc,
                        std::vector<std::vector<data_cell>> &lidar_cells,
                        std::vector<LidarObject> &lidar_objs);

        private:
            int GridCell(const pcl::PointCloud<XYZI_1>::Ptr &data_in);
            int PushCell( Localization &loc,
                         std::vector<std::vector<data_cell>> &lidar_cells,
                         std::vector<LidarObject> &lidar_objs);
            void CoordinateTransformation(std::vector<LidarObject> &cells,
                                          Localization &loc);
            std::vector<std::vector<data_cell>> data_cell_tmp;
            std::vector<std::vector<data_cell>> data_cell_;
            std::shared_ptr<LidarCellsGrid> lidar_cells_grid_;
            // std::shared_ptr<DBSCAN> dbscan_;
            std::shared_ptr<Classify> classify_;
            std::shared_ptr<Track> track_;

            rclcpp::Node *private_nh_;
            std::vector<pcl::PointCloud<pcl::PointXYZI>> clusters;

            int row;
            int col;
            int hz;
            double time;
            cell_config cell_config_;
        };
        // #define INSTANTIATE_LidarCells(T1, std::vector<std::vector<data_cell>>,
        // cell_config) template class LidarCells<T1,
        // std::vector<std::vector<data_cell>>, cell_config>;

    } // namespace algorithm
} // namespace perception
#endif